#include <cfloat>
#include <cmath>
#include <cassert>
#include "Circumcenter.h"

Vector circumCenter0(const Vector& p1, const Vector& p2, const Vector& p3);

int circumCenter1(const Vector &s1, const Vector &s2, const Vector& p2, const Vector& p3, Vector* c1);

Vector circumCenter2(const Vector &s1, const Vector& p2, const Vector& p3);//

int circumCenter3(const Vector &s1, const Vector& p2, const Vector &s3, const Vector &s4, Vector* vs);//

int circumCenter4(const Vector &s1, const Vector& s2, const Vector& s3, const Vector &s4, const Vector &p, Vector* c1);

int circumCenter5(const Vector &s1, const Vector& s2, const Vector& s3, const Vector &s4, const Vector &s5, const Vector &s6,
				  Vector* cs, double *rs);

bool intersectLine(const Vector& p0, const Vector& v0, const Vector& p1, const Vector& v1, Vector& p)
{
	double d = v0.Cross(v1);
	if(equalDouble(d, 0.0))
		return false;
	double c0 = v0.Dot(p0);
	double c1 = v1.Dot(p1);
	p.x = (c0*v1.y - c1*v0.y) / d;
	p.y = (c1*v0.x - c0*v1.x) / d;
	return true;
}

int circumCenter(const Site& s1, const Site& s2, const Site& s3, Vector* cs, double* rs)
{
	int bst[3];
	Site sid[3], tmp;
	Vector p1, p2, p3, p4;

	if(s1.id_==s2.id_ || s1.id_==s3.id_ || s2.id_==s3.id_)
		return 0;

	sid[0] = s1;
	sid[1] = s2;
	sid[2] = s3;
	bst[0] = sid[0].isPoint_ ? 1 : 0;
	bst[1] = sid[1].isPoint_ ? 1 : 0;
	bst[2] = sid[2].isPoint_ ? 1 : 0;

	int sum = bst[0] + bst[1] + bst[2];

	switch(sum){
		case 3:
			{
				p1 = sid[0].p0_;
				p2 = sid[1].p0_;
				p3 = sid[2].p0_;
				double c = (p1-p2).Cross(p2-p3);
				if (equalDouble(c, 0.0))
					return 0;
				cs[0] = circumCenter0(p1, p2, p3);
			}
			break;
		case 2:
			if(!s1.isPoint_){
				if(s1.p0_==s2.p0_)
				{
					p1 = s1.p1_; p2 = s2.p0_; p3 = s3.p0_;
				}else if(s1.p1_==s2.p0_)
				{
					p1 = s1.p0_; p2 = s2.p0_; p3 = s3.p0_;
				}else if(s1.p0_==s3.p0_)
				{
					p1 = s1.p1_; p2 = s3.p0_; p3 = s2.p0_;
				}else if(s1.p1_==s3.p0_)
				{
					p1 = s1.p0_; p2 = s3.p0_; p3 = s2.p0_;
				}else{
					int n = circumCenter1(s1.p0_, s1.p1_, s2.p0_, s3.p0_, cs);
					for(int i=0; i < n; i ++) rs[i] = (cs[i]-s2.p0_).getLength();
					return n;
				}
			}else if(!s2.isPoint_){
				if(s2.p0_==s1.p0_)
				{
					p1 = s2.p1_; p2 = s1.p0_; p3 = s3.p0_;
				}else if(s2.p1_==s1.p0_)
				{
					p1 = s2.p0_; p2 = s1.p0_; p3 = s3.p0_;
				}else if(s2.p0_==s3.p0_)
				{
					p1 = s2.p1_; p2 = s3.p0_; p3 = s1.p0_;
				}else if(s2.p1_==s3.p0_)
				{
					p1 = s2.p0_; p2 = s3.p0_; p3 = s1.p0_;
				}else{
					int n = circumCenter1(s2.p0_, s2.p1_, s1.p0_, s3.p0_, cs);
					for(int i=0; i < n; i ++) rs[i] = (cs[i]-s1.p0_).getLength();
					return n;
				}
			}else if(!s3.isPoint_){
				if(s3.p0_==s1.p0_)
				{
					p1 = s3.p1_; p2 = s1.p0_; p3 = s2.p0_;
				}else if(s3.p1_==s1.p0_)
				{
					p1 = s3.p0_; p2 = s1.p0_; p3 = s2.p0_;
				}else if(s3.p0_==s2.p0_)
				{
					p1 = s3.p1_; p2 = s2.p0_; p3 = s1.p0_;
				}else if(s3.p1_==s2.p0_)
				{
					p1 = s3.p0_; p2 = s2.p0_; p3 = s1.p0_;
				}else{
					int n = circumCenter1(s3.p0_, s3.p1_, s1.p0_, s2.p0_, cs);
					for(int i=0; i < n; i ++) rs[i] = (cs[i]-s1.p0_).getLength();
					return n;
				}
			}
			cs[0] = circumCenter2(p1, p2, p3);
			break;
		case 1:
			if(s1.isPoint_){
				p2 = s1.p0_;
				if(p2==s2.p0_)
				{
					p1 = s2.p1_; p3 = s3.p0_; p4 = s3.p1_;
				}else if(p2==s2.p1_)
				{
					p1 = s2.p0_; p3 = s3.p0_; p4 = s3.p1_;
				}else if(p2==s3.p0_)
				{
					p1 = s3.p1_; p3 = s2.p0_; p4 = s2.p1_;
				}else if(p2==s3.p1_)
				{
					p1 = s3.p0_; p3 = s2.p0_; p4 = s2.p1_;
				}else{
					int n = circumCenter4(s2.p0_, s2.p1_, s3.p0_, s3.p1_, s1.p0_, cs);
					for(int i=0; i < n; i ++) rs[i] = (cs[i]-s1.p0_).getLength();
					return n;
				}
			}else if(s2.isPoint_){
				p2 = s2.p0_;
				if(p2==s1.p0_)
				{
					p1 = s1.p1_; p3 = s3.p0_; p4 = s3.p1_;
				}else if(p2==s1.p1_)
				{
					p1 = s1.p0_; p3 = s3.p0_; p4 = s3.p1_;
				}else if(p2==s3.p0_)
				{
					p1 = s3.p1_; p3 = s1.p0_; p4 = s1.p1_;
				}else if(p2==s3.p1_)
				{
					p1 = s3.p0_; p3 = s1.p0_; p4 = s1.p1_;
				}else{
					int n = circumCenter4(s1.p0_, s1.p1_, s3.p0_, s3.p1_, s2.p0_, cs);
					for(int i=0; i < n; i ++) rs[i] = (cs[i]-s2.p0_).getLength();
					return n;
				}
			}else if(s3.isPoint_){
				p2 = s3.p0_;
				if(p2==s1.p0_)
				{
					p1 = s1.p1_; p3 = s2.p0_; p4 = s2.p1_;
				}else if(p2==s1.p1_)
				{
					p1 = s1.p0_; p3 = s2.p0_; p4 = s2.p1_;
				}else if(p2==s2.p0_)
				{
					p1 = s2.p1_; p3 = s1.p0_; p4 = s1.p1_;
				}else if(p2==s2.p1_)
				{
					p1 = s2.p0_; p3 = s1.p0_; p4 = s1.p1_;
				}else{
					int n = circumCenter4(s1.p0_, s1.p1_, s2.p0_, s2.p1_, s3.p0_, cs);
					for(int i=0; i < n; i ++) rs[i] = (cs[i]-s3.p0_).getLength();
					return n;
				}
			}
			{
				int n = circumCenter3(p1, p2, p3, p4, cs);
				for(int i=0; i < n; i ++) rs[i] = (cs[i]-p2).getLength();
				return n;
			}
			break;
		case 0:
			{
				int n = circumCenter5(s1.p0_, s1.p1_, s2.p0_, s2.p1_, s3.p0_, s3.p1_, cs, rs);
				return n;
			}
		default:
			assert(NULL);
	}

	if(cs[0].x==DBL_MAX) return 0;
	else rs[0] = (cs[0]-p2).getLength();
	return 1;
}

Vector
circumCenter0(const Vector &p1, const Vector& p2, const Vector& p3)
{
	Vector rv;
	Vector m12 = (p1 + p2) / 2;
	Vector m13 = (p1 + p3) / 2;

	if(!intersectLine(m12, p1 - p2, m13, p1 - p3, rv))
		return Vector(DBL_MAX, DBL_MAX);
	return rv;
}

int
circumCenter1(const Vector &s1, const Vector &s2, const Vector& p2, const Vector& p3, Vector* cs)
{
	Vector s12 = s1 - s2;
	Vector s34 = p2 - p3;
	Vector pbar = (p2 + p3) / 2;

	double cr1 = (s1-p2).Cross(s2-p2);
	double cr2 = (s1-p3).Cross(s2-p3);
	if((cr1<0 && cr2>0) || (cr1>0 && cr2<0))
		return 0;

	Vector origin;
	if(!intersectLine(s1, Vector(s12.y, -s12.x), p2, Vector(s34.y, -s34.x), origin))
	{
		Vector dp = s12*((pbar-s2).Dot(s12)/s12.getSquaredLength()) + s2;
		double len = (dp - pbar).getLength();
		if(equalDouble(len, 0.0))
			return 0;
		double r = len/2 - s34.getSquaredLength()/(8*len);
		cs[0] = (pbar*(len-r) + dp*r)/len;
		return 1;
	}

	double slen = (pbar-origin).getSquaredLength() - s34.getSquaredLength()/4;
	if(slen < 0)
		return 0;
	double r = sqrt(slen);
	double l1 = (s1-origin).getLength();
	if(equalDouble(l1, 0.0))
	{
		intersectLine(pbar, s34, s1, s12, cs[0]);
		return 1;
	}
	Vector dp = (origin*(r+l1) - s1*r) / l1;
	intersectLine(pbar, s34, dp, s12, cs[0]);
	
	dp = (origin*(l1-r) + s1*r) / l1;
	intersectLine(pbar, s34, dp, s12, cs[1]);

	return 2;
}

Vector
circumCenter2(const Vector &s1, const Vector& p2, const Vector& p3)
{
	Vector rv;
	Vector v23 = p2 - p3;
	Vector vbar = (p2+p3) / 2;

	if(!intersectLine(vbar, v23, p2, s1 - p2, rv))
		return Vector(DBL_MAX, DBL_MAX);

	return rv;
}

int
circumCenter3(const Vector &s1, const Vector& p2, const Vector &s3, const Vector &s4, Vector* vs)
{
	Vector dv;
	Vector v12 = s1 - p2;
	Vector v34 = s3 - s4;
	double c1 = v12.Cross(v34);
	double d1 = v12.Dot(v34);

	if(!intersectLine(p2, v12, s3, Vector(v34.y, -v34.x), dv))
	{
		Vector dp = v34*((p2-s3).Cross(p2-s4)/v34.getSquaredLength());
		vs[0] = p2 + dp;
		vs[1] = p2 - dp;
		return 2;
	}

	if(equalDouble(c1, 0.0))
	{
		vs[0] = (p2 + dv) / 2;
		return 1;
	}else{
		double l1 = v12.getLength();
		double l2 = v34.getLength();
		double r = d1 / (l1*l2);
		vs[0] = (dv*r+p2)/(1+r);
		vs[1] = (p2-dv*r)/(1-r);
		return 2;
	}
}

int circumCenter4(const Vector &s1, const Vector& s2, const Vector& s3, const Vector &s4, const Vector &p, Vector* cs)
{
	Vector origin;
	Vector v12 = s1 - s2;
	Vector v34 = s3 - s4;

	if(!intersectLine(s1, Vector(v12.y, -v12.x), s3, Vector(v34.y, -v34.x), origin))
	{
		Vector p12, p34, pbar;
		intersectLine(s1, Vector(v12.y, -v12.x), p, v12, p12);
		intersectLine(s3, Vector(v34.y, -v34.x), p, v34, p34);
		pbar = (p12 + p34) / 2;
		double d = (p12-p).Dot(p34-p);
		if(equalDouble(d, 0.0))
		{
			cs[0] = pbar;
			return 1;
		}else if(d > 0)
			return 0;
		double l1 = (p12-p).getLength();
		double l2 = (p34-p).getLength();
		double r = (p12-p34).getLength();
		double len = sqrt(r*r-(l1-l2)*(l1-l2)/4);
		v12.normalize();
		v12 = v12*len;
		cs[0] = pbar + v12;
		cs[1] = pbar - v12;
		return 2;
	}

	Vector po = p - origin;
	Vector p12 = v12*(po.Dot(v12) / v12.getSquaredLength());
	Vector p34 = v34*(po.Dot(v34) / v34.getSquaredLength());
	double l1 = p12.getLength();
	double l2 = fabs(po.Cross(v12) / v12.getLength());
	p12 = p12 / l1;
	p34.normalize();
	double cos_ = p12.Dot(p34);
	double tan2 = (1-cos_) / (1+cos_);
	double tan_ = sqrt(tan2);
	double b = l1 + l2*tan_;
	double c = l1*l1 + l2*l2;
	double delta = b*b - c;
	if(equalDouble(delta, 0.0))
	{
		intersectLine(p12*b+origin, v12, p34*b+origin, v34, cs[0]);
		return 1;
	}
	delta = sqrt(delta);

	double r = b+delta;
	intersectLine(p12*r+origin, v12, p34*r+origin, v34, cs[0]);

	r = b-delta;
	intersectLine(p12*r+origin, v12, p34*r+origin, v34, cs[1]);

	return 2;
}

int circumCenter5(const Vector &s1, const Vector& s2, const Vector& s3, const Vector &s4, const Vector &s5, const Vector &s6,
				  Vector* cs, double* rs)
{
	bool b1=true ,b2 = true, b3 = true;
	Vector v1, v2, v3;
	Vector v12 = s1 - s2;
	Vector v34 = s3 - s4;
	Vector v56 = s5 - s6;
	if(!intersectLine(s1, Vector(v12.y, -v12.x), s3, Vector(v34.y, -v34.x), v1))
	{
		b1 = false;
	}
	if(!intersectLine(s3, Vector(v34.y, -v34.x), s5, Vector(v56.y, -v56.x), v2))
	{
		b2 = false;
	}
	if(!intersectLine(s1, Vector(v12.y, -v12.x), s5, Vector(v56.y, -v56.x), v3))
	{
		b3 = false;
	}
	if(!b1 && !b2 && !b3) return 0;

	if(!b1)
	{
		double l12 = v12.getLength();
		double len = (v2-v3).getLength();
		double b = (v2-v3).Dot(v12) / l12;
		double l1 = (len + b) / 2;
		double l2 = (len - b) / 2;
		Vector dp1 = v12*(l1/l12);
		Vector dp2 = v12*(l2/l12);
		cs[0] = ((v2+dp2) + (v3+dp1)) / 2;
		rs[0] = sqrt(l1*l2);
		cs[1] = ((v2-dp1) + (v3-dp2)) / 2;
		rs[1] = rs[1];
		return 2;
	}

	if(!b2)
	{
		double l34 = v34.getLength();
		double len = (v1-v3).getLength();
		double b = (v1-v3).Dot(v34) / l34;
		double l1 = (len + b) / 2;
		double l2 = (len - b) / 2;
		Vector dp1 = v34*(l1/l34);
		Vector dp2 = v34*(l2/l34);
		cs[0] = ((v1+dp2) + (v3+dp1)) / 2;
		rs[0] = sqrt(l1*l2);
		cs[1] = ((v1-dp1) + (v3-dp2)) / 2;
		rs[1] = rs[0];
		return 2;
	}

	if(!b3)
	{
		double l12 = v12.getLength();
		double len = (v1-v2).getLength();
		double b = (v1-v2).Dot(v12) / l12;
		double l1 = (len + b) / 2;
		double l2 = (len - b) / 2;
		Vector dp1 = v12*(l1/l12);
		Vector dp2 = v12*(l2/l12);
		cs[0] = ((v1+dp2) + (v2+dp1)) / 2;
		rs[0] = sqrt(l1*l2);
		cs[1] = ((v1-dp1) + (v2-dp2)) / 2;
		rs[1] = rs[0];
		return 2;
	}

	double l12 = (v1-v2).getLength();
	double l23 = (v2-v3).getLength();
	double l13 = (v1-v3).getLength();
	double len = (l12 + l23 + l13) / 2;
	double l1 = len - l23;
	double l2 = len - l13;
	double l3 = len - l12;

	Vector m12 = (v1*l2+v2*l1)/l12;
	Vector m13 = (v1*l3+v3*l1)/l13;
	intersectLine(m12, v34, m13, v12, cs[0]);
	rs[0] = (cs[0]-m12).getLength();

	l1 = len;
	l2 = len - l12;
	l3 = len - l13;
	m12 = (v2*l1-v1*l2)/l12;
	m13 = (v3*l1-v1*l3)/l13;
	intersectLine(m12, v34, m13, v12, cs[1]);
	rs[1] = (cs[1]-m12).getLength();

	l1 = len - l12;
	l2 = len;
	l3 = len - l23;
	m12 = (v1*l2-v2*l1)/l12;
	m13 = (v1*l3+v3*l1)/l13;
	intersectLine(m12, v34, m13, v12, cs[2]);
	rs[2] = (cs[2]-m12).getLength();

	l1 = len - l13;
	l2 = len - l23;
	l3 = len;
	m12 = (v1*l2+v2*l1)/l12;
	m13 = (v1*l3-v3*l1)/l13;
	intersectLine(m12, v34, m13, v12, cs[3]);
	rs[3] = (cs[3]-m12).getLength();

	return 4;
}
